



#include "stdafx.h"
#include "MoveController.h"





Move::Move()
:m_nCurMoveType(MT_WALK), 
m_nPrevMoveState(UnitBase::ums_Idle), 
m_bWalkOnly(false),
m_MaxSpeed(MAX_SPEED), 
m_MaxTurn ( MAX_TURN ), 
m_AngularSpeed (ANGULAR_SPEED)
{
}

Move::~Move()
{
}

bool 
Move::OnCreate(StaticUnitGene* pGSData, IGeneData* pGDData)
{
	if (__super::OnCreate(pGSData, pGDData))
	{
		int nData;
		nData = m_pStaticData->GetArg(AT_MaxSpeed);
		if (nData!=0)
		{
			m_MaxSpeed = float(nData) / 1000;
		}

		nData = m_pStaticData->GetArg(AT_MaxTurn);
		if (nData!=0)
		{
			m_MaxTurn = float(nData) / 1000;
		}

		nData = m_pStaticData->GetArg(AT_ArgularSpeed);
		if (nData!=0)
		{
			m_AngularSpeed = float(nData) / 1000;
		}
	}

	return true;
}



bool
Move::OnAttach()
{
	m_pUnit->RegGeneEvt( UGE_Move, this);
	m_pUnit->RegGeneEvt( UGE_Stop, this );

	if (m_pUnit->IsMonster())
	{
		m_bWalkOnly = true;
	}

	return true;
}

bool
Move::OnDetach()
{
	m_vctPath.clear();
	m_nPathIndex = 0;
	
	OnMoveStop();

	m_pUnit->UnregGeneEvt( UGE_Move, this);
	m_pUnit->UnregGeneEvt( UGE_Stop, this );
	return true;
}

bool 
Move::OnUpdate(int nElapse)
{
	if (m_nPrevMoveState!=m_pUnit->GetMoveState())
	{
		//
		//	someone else change the unit move state
		if (m_nPrevMoveState==UnitBase::ums_Moving && 
			m_pUnit->GetMoveState()!=UnitBase::ums_OutOfControl)
		{
			OnMoveStop(false, false);
		}

		//
		//	keep state in sync with unit move state
		m_nPrevMoveState = m_pUnit->GetMoveState();
	}

	if (m_nPrevMoveState==UnitBase::ums_Idle ||
		m_nPrevMoveState==UnitBase::ums_Freeze)
	{
		return true;
	}

	Vector3 vct3Current = ToVector3(m_pUnit->GetPosition());
	Vector3 vct3Advance = ToVector3(m_pUnit->GetDirection());
	Vector3 vct3Position;
	Vector3 vct3DiffToNextPt;
	Vector3 vct3CurrentDiff;
	Real    nCurrDiff;
	Real    nNewDiff;
	Scene*  pScene;


	if (m_vctPath.size() == 0)
	{
		return true;
	}

	// Always go into idle to allow blending

	pScene = m_pUnit->GetScene();

	if (pScene == NULL)
	{
		return true;
	}

	//m_MaxSpeed = pScene->GetMaxSpeed();

	//start to move
	if (m_pUnit->GetVelocity() == 0.0f)
	{
		m_pUnit->SetVelocity(0.01f);  // Just so Idle stops
		m_nAcceleration = 1.0f;   
	}
	else 
	{
		// Check to make sure the desired and actual directions match so the unit doesn't make a wide turn on startup which can cause collision issues, remove check if that is ok
		//    if (m_pUnit->GetVelocity() < m_pu3dMoveAnimation->m_nMaxRange) // && m_pUnit->m_lmUnit.GetDirectionsMatch() == true)
		if (m_nAcceleration != 0.0f)//moving
		{
			float nVelocity;

			nVelocity = m_pUnit->GetVelocity() * m_MaxSpeed/* * 0.75*/;

			nVelocity += (m_MaxSpeed * (float(nElapse)) / 1000 * m_nAcceleration) / 0.5;

			// Check ranges and stop
			if (nVelocity >= m_MaxSpeed)//to max speed
			{
				m_pUnit->SetVelocity(1.0);

				m_nAcceleration = 0.0f;
			}
			else if (nVelocity <= 0.0f)//stop moving
			{
				m_pUnit->SetVelocity(0.0f);
				m_nAcceleration = 0.0f;

				OnMoveStop();

				m_vctPath.clear();
			}
			else//
			{
				if (nVelocity > 0.0f)
					m_pUnit->SetVelocity(nVelocity/m_MaxSpeed);
				//          TRACE("Move Weight %f\n", nWeight);
			}
		}

		// If the unit is turning, make them turn                                                   
		if (m_pUnit->m_lmUnit.GetDirectionsMatch() == false)
		{
			Matrix3 mtx3AngularMo;
			Vector3 vct3NewDirection;
			Vector3 vct3Base(0.0f, 0.0f, -1.0f);
			float nVectorAngle;
			float nOrigVectorAngle;
			float nNewVectorAngle;
			Vector2Ex  vct2Direction(vct3Advance.x, -vct3Advance.z);
			if(!m_vctPath.empty() && 0 <= m_nPathIndex && m_nPathIndex < (int)m_vctPath.size())
			{
				m_pUnit->m_lmUnit.SetDesiredDirection(m_vctPath[m_nPathIndex] - ToAD_Vector3(vct3Current));
			}


			Vector2Ex  vct2DesiredDirection(m_pUnit->m_lmUnit.m_vct3DesiredDirection.x, -m_pUnit->m_lmUnit.m_vct3DesiredDirection.z);

			float nAngularInc = 0;

			nAngularInc = (float(nElapse)) / 1000 * m_AngularSpeed;

			// Setup the vectors and normalize them
			vct2Direction.normalise();
			vct2DesiredDirection.normalise();

			// Get the angle between the two vectors
			nOrigVectorAngle = vct2DesiredDirection.GetAngleBetween(vct2Direction); 

			// If the desired direction is clockwise to the current direction spin counter clockwise to rotate along shortest angle
			if (vct2Direction.GetIsClockwise(vct2DesiredDirection) == true)
				nOrigVectorAngle = -nOrigVectorAngle;

			//       TRACE("Pre: nIndex %d Angle %f ", m_nPathIndex, nOrigVectorAngle);

			// Cap the maximum turn amount
			nVectorAngle = max(-m_MaxTurn, min(m_MaxTurn, nOrigVectorAngle * nAngularInc));

			//       TRACE("Pst: nIndex %d Angle %f ", m_nPathIndex, nVectorAngle);

			// Setup the rotation
			mtx3AngularMo.FromAxisAngle(Vector3(0, 1, 0), (Radian)nVectorAngle);
			Matrix4 mtx4AngularMo(mtx3AngularMo);

			// Rotate the current direction
			vct3NewDirection = mtx4AngularMo * vct3Advance;

			vct2Direction.x = vct3NewDirection.x;
			vct2Direction.y = -vct3NewDirection.z;
			vct2Direction.normalise();

			// Test out the new angle to see if it is farther away to signify the rotation is complete
			nNewVectorAngle = vct2DesiredDirection.GetAngleBetween(vct2Direction); 
			if (vct2Direction.GetIsClockwise(vct2DesiredDirection) == true)
				nNewVectorAngle = -nNewVectorAngle;

			//       TRACE("Angle: Pre %f Post %f\n", nOrigVectorAngle, nNewVectorAngle);
			//       if (nNewVectorAngle > nOrigVectorAngle)
			// Crossed zero, done turning
			if (fabsf(nOrigVectorAngle - nNewVectorAngle) > fabsf(nOrigVectorAngle)) 
			{
				vct3NewDirection = ToVector3(m_pUnit->m_lmUnit.m_vct3DesiredDirection);

				nNewVectorAngle = 0.0f; 
				//          TRACE("Cmp: %d\n", m_nPathIndex);
			}

			if(!m_vctPath.empty() && 0 <= m_nPathIndex && m_nPathIndex < (int)m_vctPath.size())
			{
				AD_Vector3 vec3Tmp = m_vctPath[m_nPathIndex] - ToAD_Vector3(vct3Current);
				if (ToVector3(vec3Tmp).length() < 10)       //
				{
					vct3NewDirection = ToVector3(vec3Tmp);
				}
			}



			// Set the current direction
			m_pUnit->SetDirection(ToAD_Vector3(vct3NewDirection));

			// Stop if turn is completed (ie we turned less than the max)
			if (nNewVectorAngle == 0.0f)
			{
				m_pUnit->m_lmUnit.SetDirectionsMatch(false);
			}
		}


		// Find out why statement below is needed, needed for when a scene is switched
		if (m_pUnit->GetVelocity() > 0 && m_nPathIndex + 1 <= (int)m_vctPath.size())
		{
			Real nInc = (float(nElapse)) / 1000 * (m_pUnit->GetVelocity() * m_MaxSpeed); 

			//       TRACE("TimePos: %f\n", pAnimationState->getTimePosition());

			vct3Position = ToVector3(m_pUnit->GetPosition());

			// Get Current distance to next waypoint
			vct3CurrentDiff.x = m_vctPath[m_nPathIndex].x - vct3Position.x;
			vct3CurrentDiff.y = m_vctPath[m_nPathIndex].y - vct3Position.y;
			vct3CurrentDiff.z = m_vctPath[m_nPathIndex].z - vct3Position.z;
			nCurrDiff = vct3CurrentDiff.squaredLength();

			// Advance the character
			vct3Position = vct3Current + vct3Advance * nInc;

			if (!pScene->GetTilingGrid().GetIsBlockPosition(ToAD_Vector3(vct3Position)))
			{
				m_pUnit->SetPosition(ToAD_Vector3(vct3Position));
			}
			else if (!pScene->GetIsValidPosition(ToAD_Vector3(vct3Position)))
			{
				TilingGrid & tg = pScene->GetTilingGrid();
				int width = tg.GetGridWidth();
				int depth = tg.GetGridDepth();
				const float diff = 0.1f;
				if (vct3Position.x <= 0.0)
				{
					vct3Position.x = 0.0 + diff;
				}
				else if (vct3Position.x >= width)
				{
					vct3Position.x = width - diff;
				}
				if (vct3Position.z <= 0.0)
				{
					vct3Position.z = 0.0 + diff;
				}
				else if (vct3Position.z >= depth)
				{
					vct3Position.z = depth - diff;
				}
			}
			else if (pScene->GetTilingGrid().GetIsBlockPosition(ToAD_Vector3(vct3Position)))
			{
				TilingGrid & tg = pScene->GetTilingGrid();
				float width = tg.GetTileWidth();
				float depth = tg.GetTileDepth();

				TilePos nTileX_old;
				TilePos nTileZ_old;
				tg.GridPixelsToTilePosition(m_pUnit->GetPosition(), nTileX_old, nTileZ_old);
				TilePos nTileX_new;
				TilePos nTileZ_new;
				tg.GridPixelsToTilePosition(ToAD_Vector3(vct3Position), nTileX_new, nTileZ_new);

				if ((int)nTileX_old-(int)nTileX_new > 1 ||
					(int)nTileX_old-(int)nTileX_new < -1 ||
					(int)nTileZ_old-(int)nTileZ_new > 1 ||
					(int)nTileZ_old-(int)nTileZ_new < -1)
				{
					AD_WARN("ERROR!! Move over 1 tile next frame\n");
				}
				else
				{
					Vector3 adjust = vct3Position; 
					if (tg.GetIsBlockPt((int)nTileX_new,(int)nTileZ_old))
					{
						int x = (int)nTileX_old>(int)nTileX_new?(int)nTileX_old:(int)nTileX_new;
						adjust.x = x*tg.GetTileWidth();
						adjust.x += (int)nTileX_old>(int)nTileX_new?0.1:-0.1;
					}
					if (tg.GetIsBlockPt((int)nTileX_old,(int)nTileZ_new))
					{
						int z = (int)nTileZ_old>(int)nTileZ_new?(int)nTileZ_old:(int)nTileZ_new;
						adjust.z = z*tg.GetTileDepth();
						adjust.z += (int)nTileZ_old>(int)nTileZ_new?0.1:-0.1;
					}
					vct3Position = adjust;
				}
			}
			if (pScene->GetIsValidPosition(ToAD_Vector3(vct3Position)) &&
				!pScene->GetTilingGrid().GetIsBlockPosition(ToAD_Vector3(vct3Position)))
			{			
				m_pUnit->SetPosition(ToAD_Vector3(vct3Position));
			}

			//Move chat bubble.
			MoveChatBubble();
			//       m_pScene->GetTilingGrid().GetGridYPositionInPixels(vct3Position.x, vct3Position.z, vct3Position.y);

			// Get new distance to way 
			vct3DiffToNextPt.x = m_vctPath[m_nPathIndex].x - vct3Position.x;
			vct3DiffToNextPt.y = m_vctPath[m_nPathIndex].y - vct3Position.y;
			vct3DiffToNextPt.z = m_vctPath[m_nPathIndex].z - vct3Position.z;
			nNewDiff = vct3DiffToNextPt.squaredLength();

			//       TRACE("Index %d/%d Curr %f Diff %f\n", m_nPathIndex, m_vctPath.size(), nCurrDiff, nNewDiff);
			//       TRACE("            CX %f CY %f CZ %f NX %f NY %f NZ %f\n", vct3Position.x, vct3Position.y, vct3Position.z, m_vctPath[m_nPathIndex].x, m_vctPath[m_nPathIndex].y, m_vctPath[m_nPathIndex].z);


			Vector3 vct3ToEnd;
			int vectorSize = m_vctPath.size();
			vct3ToEnd.x = m_vctPath[vectorSize-1].x - vct3Position.x;
			vct3ToEnd.y = m_vctPath[vectorSize-1].y - vct3Position.y;
			vct3ToEnd.z = m_vctPath[vectorSize-1].z - vct3Position.z;
			Real nToEndDiff = vct3ToEnd.squaredLength();
			Real SlowDownLength = 1*pScene->GetTileDepth();
			if (nToEndDiff < SlowDownLength*SlowDownLength)
			{
				m_pUnit->SetVelocity(0.5);
			}
			// Get ready to stop
			if (nNewDiff < 1.40f || (vct3CurrentDiff.dotProduct(vct3DiffToNextPt) < 0.0f)) // && nNewDiff < 1000.0f))
			{
				// If at the end, stop
				if (m_nPathIndex + 1 >= (int)m_vctPath.size())
				{
					m_pUnit->SetVelocity(0.0f);
					m_nAcceleration = 0.0f;

					OnMoveStop(true);

					m_vctPath.clear();
				}
				else
				{
					if (m_nPathIndex + 1 < (int)m_vctPath.size())
					{
						m_nPathIndex++;
						SetDesiredUnitOrientation(ToVector3(m_pUnit->GetPosition()), ToVector3(m_vctPath[m_nPathIndex]));
					}
				}
			}
		}
	}
	return true;
}

void Move::OnHook(GeneEvt& evt)
{
	if (m_pUnit->GetMoveState()!=UnitBase::ums_Freeze)
	{
		if (evt.GetID()==UGE_Move)
		{
			TilePos fX = evt.GetArgX()/1000;
			TilePos fY = evt.GetArgY()/1000;

			if (!m_bWalkOnly &&  m_nCurMoveType != evt.GetArgZ())
			{
				m_nCurMoveType = evt.GetArgZ();

				int	nNewID = m_pStaticData->GetArg(AT_LinkToMove+m_nCurMoveType);

				static IStaticDataLoader* s_geneLoader = NULL;
				if (s_geneLoader==NULL)
				{
					s_geneLoader = g_StaticDataFactory->GetLoader(StaticUnitGene::category);
				}

				if  (s_geneLoader)
				{
					StaticUnitGene* pNewSD = dynamic_cast<StaticUnitGene*>(s_geneLoader->Get(nNewID));
					if (pNewSD)
					{
						OnCreate(pNewSD);
					}
				}
			}

			DeterminePath(fX, fY);
		}
	}

	if ( UGE_Stop == evt.GetID() )
	{
		m_vctPath.clear();
		m_nPathIndex = 0;

		if (evt.GetArgX()!=0)
		{
			//
			//	this is a stop that will freeze current unit
			//	unit in freeze mode can't move to other place
			//
			OnMoveStop(true, true);
		}
		else
		{
			OnMoveStop();
		}
	}
}



bool Move::DeterminePath(TilePos nXDest, TilePos nZDest)//Scene* pScene, TilePos nXDest, TilePos nZDest, DWORD nTick)
{
	if(NULL == m_pUnit)
		return false;

	Scene * pScene = m_pUnit->GetScene();
	m_nPathIndex = 0;
	bool ret = false;
	if (m_pUnit && pScene != NULL)
	{
		//ScenePath spUnit(pScene);
		TilePos  nXStart;
		TilePos  nZStart;
		Vector3  vct3Grid;
		Vector3  vctPrevious;
		CPoint   ptGrid;

		m_pUnit->GetTilePosition(nXStart, nZStart);

		if ((int)nXStart == (int)nXDest &&
			(int)nZStart == (int)nZDest)
		{
			return false;
		}

		m_nAcceleration = 1.0f;
		m_vctPath.clear();

		AStartPath AstartPath(pScene);
		AstartPath.GetPath(m_vctPath, CPoint(nXStart, nZStart), CPoint(nXDest, nZDest));

		m_nPathIndex = 1;
		// If a path convert to real pixels
		if (m_vctPath.size() > 0)
		{
			if (m_vctPath.size() >= 2)
			{
				SetDesiredUnitOrientation(ToVector3(m_vctPath[0]), ToVector3(m_vctPath[1]));
			}

			if (m_Target.x > 0.0 && m_Target.z > 0.0)//m_Target has been set
			{
				vctPrevious = ToVector3(m_vctPath[m_vctPath.size()-1]);

				pScene->GetTilingGrid().GridToPixels(m_Target.x, m_Target.z, ToAD_Vector3(vct3Grid));
				float dx = vct3Grid.x - vctPrevious.x;
				float dz = vct3Grid.z - vctPrevious.z;
				if (dx<pScene->GetTileWidth() && dx>0.0 && dz<pScene->GetTileDepth()  && dz>0.0)
				{
					m_vctPath[m_vctPath.size()-1] = ToAD_Vector3(vct3Grid);
				}
			}
			m_pUnit->m_lmUnit.SetDirectionsMatch(false);
			ret = true;
			OnMoveStart();
		}
		else
		{
			OnMoveStop();
		}
	}
	else
	{
		ret = false;
	}


	return ret;
}


/**
* Sets the orientation so the unit faces the next position
*
* @param vct3Position  Reference or current position
* @param vct3NextPosition Next position
*/

bool Move::SetDesiredUnitOrientation(const Vector3& vct3Position, const Vector3& vct3NextPosition)
{
	Vector3 vct3DesiredDir(vct3NextPosition.x - vct3Position.x, vct3NextPosition.y - vct3Position.y, vct3NextPosition.z - vct3Position.z);
	Vector3 vct3Dir;

	m_pUnit->m_lmUnit.SetDesiredDirection(ToAD_Vector3(vct3DesiredDir));

	// Get current direction and see if they match
	vct3Dir = ToVector3(m_pUnit->m_lmUnit.m_vct3Direction);

	vct3DesiredDir.normalise();
	vct3Dir.normalise();

	// find angle between
	float nAngle = vct3DesiredDir.dotProduct(vct3Dir);

	// 1 is parallel, so give some play
	if (vct3DesiredDir.dotProduct(vct3Dir) < ALLOW_ANGLE_PLAY)
		m_pUnit->m_lmUnit.SetDirectionsMatch(false);
	else
		m_pUnit->m_lmUnit.SetDirectionsMatch(false);

	return m_pUnit->m_lmUnit.GetDirectionsMatch();
}

void Move::MoveChatBubble()
{
	UnitRole* pRole = dynamic_cast<UnitRole*>(m_pUnit);
	if(NULL == pRole)
	{
		return ;
	}

	Scene* pScene = pRole->GetScene();

	//Change the position to screen from world.
	ControlWorld2Screen  w2s;
	ObsEvtDefault evt(Ob_Evt_Control_GetCeilingCenter,  &w2s.vctWorld);
	pRole->Changed(evt);
	ObsEvtDefault evtX(Ob_Evt_Control_World2Screen,  &w2s);
	pScene->Changed(evtX);

	//Move chat bubble.
	ChatBubbleInfo infoData(ChatBubbleInfo::BubbleType_NormalPlayer);
	infoData.m_uiRoleId = pRole->GetRoleID();
	infoData.m_nX = WINDOW_WIDTH_NORMAL*(w2s.vctScreen.x);
	infoData.m_nY = WINDOW_HEIGHT_NORMAL*(w2s.vctScreen.y); 

	CMsgMoveChatBubble msgMove;
	msgMove.m_pChatInfo = &infoData;
	pScene->GetInputDevice()->Send(&msgMove);
}

void Move::SetMaxSpeed(float maxSpeed)
{
	m_MaxSpeed = maxSpeed;
}

float Move::GetMaxSpeed()
{
	return m_MaxSpeed;
}

void Move::OnMoveStart()
{
	m_nPrevMoveState = UnitBase::ums_Moving;
	m_pUnit->SetMoveState(m_nPrevMoveState);

	ObsEvtUSC evt(m_pStaticData->GetArg(AT_MoveState, 2));
	m_pUnit->Changed(evt);

}

void Move::OnMoveStop(bool bEndPathing /* = false */, bool bIsFreeze /* = false */)
{
	if( NULL == m_pUnit ) return;

	m_Target.x = -1.0;
	m_Target.y = -1.0;
	m_Target.z = -1.0;

	m_pUnit->SetVelocity(0.0f);
	if (bEndPathing)
	{
		CMsgMoveUnit mumBeginPathing((UnitBase*)m_pUnit);
		mumBeginPathing.m_eMoveStateType = CMsgMoveUnit::MST_ENDPATHING;
		m_pUnit->GetScene()->GetInputDevice()->Send(&mumBeginPathing);
	}

	if (!bIsFreeze)
	{
		m_nPrevMoveState = UnitBase::ums_Idle;
		m_pUnit->SetMoveState(m_nPrevMoveState);
	}
	else
	{
		m_nPrevMoveState = UnitBase::ums_Freeze;
		m_pUnit->SetMoveState(m_nPrevMoveState);
	}

	ObsEvtUSC evt(m_pStaticData->GetArg(AT_StopState, 1));
	m_pUnit->Changed(evt);
}

void Move::SetMaxTurn(float maxTurn)
{
	m_MaxTurn = maxTurn;
}

float Move::GetMaxTurn()
{
	return m_MaxTurn;
}

void Move::SetAngularSpeed(float angularSpeed)
{
	m_AngularSpeed = angularSpeed;
}

float Move::GetAngularSpeed()
{
	return m_AngularSpeed;
}
